/* Classe principale per gestire il braccio con V-REP                                                                            */
/* Tutti i file del progetto vanno salvati in V-REP_PRO_EDU/programming/                                                         */
/* Una volta compilato andare in V-REP_PRO_EDU/programming/armClient/Debug/ e copiare armClient.exe in V-REP_PRO_EDU/			 */

#include "Arm.h"
#include "Emg.h"
#include "KinectDemo.h"


using namespace std;

int main(int argc,char* argv[]){

	int portNb=0;			// Porta connessione V-REP
	int clientID;           // ID client V-REP
	int baseJoint;			// Gestore base joint
    int shoulderJoint;		// Gestore shoulder joint
    int elbowJoint;			// Gestore elbow joint
    int wristJoint;		    // Gestore wrist joint
    int right_Hand_Joint;   // Gestore right hand joint
    int left_Hand_Joint;    // Gestore left hand joint
	
	/* Gestore di un oggetto 
	int object;				
	float newPos[3];        // Nuova posizione dell'oggetto
	float eulerAngles[3];   // Nuovo orientamento dell'oggetto
	*/

	if (argc>=8){
		portNb=atoi(argv[1]);
		baseJoint=atoi(argv[2]);
		shoulderJoint=atoi(argv[3]);
		elbowJoint=atoi(argv[4]);
		wristJoint=atoi(argv[5]);
		right_Hand_Joint=atoi(argv[6]);
		left_Hand_Joint=atoi(argv[7]);
	} 
	else{
		printf("Indica i seguenti argomenti: 'portNumber baseJoint shoulderJoint elbowJoint wristJoint right_hand_Joint left_hand_Joint'!\n");
		extApi_sleepMs(5000);
		return 0;
	}

	clientID = simxStart("127.0.0.1",portNb,true,true,2000,5);
	if (clientID != -1) {

		Emg emgCtr = Emg();
		Arm armCtr = Arm(clientID, baseJoint, shoulderJoint, elbowJoint);
		Command cmd;
		KinectDemo kinectCtr = KinectDemo();
		vector<Point> path;
		armCtr.goToRestPosition();
		Sleep(3000);
	
		while (simxGetConnectionId(clientID)!=-1){

			/* Simulazione */
			
			cmd = emgCtr.getRandomEmgSignal();
			//cmd = emgCtr.getPath();

			if(cmd.getMovementType() == Command::NOTHING){						
			}
			else if(cmd.getMovementType() == Command::FLT_EST){
				path = kinectCtr.getViaPoints();
				if(!path.size()){
					armCtr.followPath(path);
				}
				else{
					armCtr.move(0, cmd.getWidth());
				}
			}
			else if(cmd.getMovementType()==Command::ABDUCTION){
				path = kinectCtr.getViaPoints();
				//printf("punto: %f punto %f punto %f", path[0].getValue(Point::X), path[1].getValue(Point::X), path[2].getValue(Point::X));
				if(path.size()){
					armCtr.followPath(path);
				}
				else{						
					armCtr.move(1, cmd.getWidth());
				}
			}
			Sleep(5000);
		}
		simxFinish(clientID);
	}
	return(0);
}